#include "gridmap.h"
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <pcl_conversions/pcl_conversions.h>
#include <QFileDialog>
#include "tools/leiliconfig.h"
#include "node/rosnode.h"
#include "util/topicfiled.h"

#include <pcl/filters/conditional_removal.h>         //条件滤波器头文件
#include <pcl/filters/passthrough.h>                 //直通滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>      //半径滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/voxel_grid.h>                  //体素滤波器头文件
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include "node/rosnode.h"
#include <QCheckBox>
#include "util/topicfiled.h"
#include "tools/map_file_manager.h"
#include "views/toast.h"
#include <QLabel>
#include "util/rosbag.h"


GridMapWidget::GridMapWidget(QWidget *parent) : QWidget(parent)
{
    setupUI();

    QObject::connect(m_btnOpenPcd,&QPushButton::clicked,this,&GridMapWidget::onOpenPCD);
    QObject::connect(m_btnConvert,&QPushButton::clicked,this,&GridMapWidget::onConvertPoint2Map);
    QObject::connect(m_btnSave,&QPushButton::clicked,this,&GridMapWidget::onSaveMap);

}

void GridMapWidget::setupUI()
{
    QVBoxLayout *vl =new QVBoxLayout(this);
    vl->setContentsMargins(0,0,0,0);

    m_rviz = new QRviz(this);
    m_rviz->Display_Grid(true, "<Fixed Frame>",QColor(160,160,164));
    m_rviz->Display_Axes(true,"<Fixed Frame>","1","0.1");
    m_rviz->DisPlay_Point2(true, QString::fromStdString(TopicFiled::pcd2point2));
    m_rviz->Display_Map(true, QString::fromStdString(TopicFiled::map), 0.7, "map");


    QHBoxLayout *hB = new QHBoxLayout(this);
    m_btnOpenPcd = new QPushButton(this);
    m_btnOpenPcd->setText("打开");

    m_btnConvert = new QPushButton(this);
    m_btnConvert->setText("转换");

    m_btnSave = new QPushButton(this);
    m_btnSave->setText("保存");

    hB->addStretch();
    hB->addWidget(m_btnOpenPcd);
    hB->addWidget(m_btnConvert);
    hB->addWidget(m_btnSave);


    QFrame *verticalLine = new QFrame(this);
    verticalLine->setFrameShape(QFrame::VLine);
    hB->addWidget(verticalLine);
    hB->addStretch();



    m_checkPoint2 =  new QCheckBox(this);
    m_checkPoint2->setObjectName("rvizCheckbox");
    m_checkPoint2->setText("三维点云");
    m_checkPoint2->setToolTip("是否显示三维点云");

    QObject::connect(m_checkPoint2,&QCheckBox::clicked, [&](bool isCheckd){
            m_rviz->Set_Enable(QString::fromStdString(TopicFiled::pcd2point2),isCheckd);

            if(isCheckd && !pc2.data.empty())
            {
                GRosNode->pubPoint2(pc2);
            }

    });

    m_checkMap =  new QCheckBox(this);
    m_checkMap->setObjectName("rvizCheckbox");
    m_checkMap->setText("珊格地图");
    m_checkMap->setToolTip("是否显示珊格地图");

    QObject::connect(m_checkMap,&QCheckBox::clicked, [&](bool isCheckd){
            m_rviz->Set_Enable(QString::fromStdString(TopicFiled::map),isCheckd);

            if(isCheckd && !m_msg.data.empty())
            {
                 GRosNode->pubMap(m_msg);
            }

    });

    m_spanMin  =  new QDoubleSpinBox(this);
    m_spanMin->setValue(LeiliCfg->getGridLimitMin());

    m_spanMax  =  new QDoubleSpinBox(this);
    m_spanMax->setValue(LeiliCfg->getGridLimitMax());

    hB->addWidget(m_spanMin);
    hB->addWidget(m_spanMax);

    hB->addWidget(m_checkPoint2);
    hB->addWidget(m_checkMap);
    hB->addSpacing(12);


    vl->addWidget(m_rviz);
    vl->addLayout(hB);
}

void GridMapWidget::initialVtkWidget()
{
    openGLWidget = new QVTKOpenGLNativeWidget(this);
    vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
    vtkSmartPointer<vtkGenericOpenGLRenderWindow> renderWindow =
        vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
    renderWindow->AddRenderer(renderer);
    view.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow,
                                                     "viewer", false));
    view->setupInteractor(openGLWidget->interactor(),
                          openGLWidget->renderWindow());
    openGLWidget->setRenderWindow(view->getRenderWindow());
}


void GridMapWidget::onOpenPCD(){
    m_fileName = QFileDialog::getOpenFileName(this, "Open Bag File", LeiliCfg->getSavePath(),
                                                    "Open PCD files(*.bag)",nullptr,QFileDialog::DontUseNativeDialog);
    if (m_fileName == ""){
        return;
    }


    if(0){
        m_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::io::loadPCDFile(m_fileName.toStdString(), *m_cloud);

        pcl::toROSMsg (*m_cloud, pc2);

        pc2.header.frame_id = "map";
    }


    bool succ = RosBagSaver::readPoint2(m_fileName.toStdString(),pc2);
    if(!succ)
    {
        Toast::toast(Toast::Warn,"打开bag文件失败!",this);
        return;
    }

    m_rviz->Set_Enable(QString::fromStdString(TopicFiled::pcd2point2),true);
    GRosNode->pubPoint2(pc2);
    m_checkPoint2->setCheckState(Qt::CheckState::Checked);

    Toast::toast(Toast::Succ, "打开bag文件成功",this);
}

void GridMapWidget::onConvertPoint2Map()
{
    if(m_cloud.get() == nullptr || m_cloud->data() == nullptr)
    {
        Toast::toast(Toast::Warn,"尚未打开点云文件!",this);
        return;
    }

    if(m_spanMin->value() >= m_spanMax->value())
    {
        Toast::toast(Toast::Warn,"点云转化二维珊格时，最小高度不能大于最大高度!请重新修改!",this);
        return;
    }

    //直通滤波：去除特定高度范围外的点（如地面以上/以下噪声），示例代码：
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(m_cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(m_spanMin->value(), m_spanMax->value()); // 保留0.3~2.0米高度的点
    pass.filter(*m_cloud);


    //体素滤波：降采样减少点云密度，提升处理效率：

    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(m_cloud);
    voxel_grid.setLeafSize(0.05f, 0.05f, 0.05f); // 5cm分辨率
    voxel_grid.filter(*m_cloud);

    //统计滤波：去除离群点，提高数据质量：
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
    sor.setInputCloud(m_cloud);
    sor.setMeanK(50); // 邻居点数
    sor.setStddevMulThresh(1.0); // 标准差倍数
    sor.filter(*m_cloud);
    //投影到二维平面
    //将点云投影到XY平面（忽略Z轴），生成二维点集：

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
    for (const auto& point : *m_cloud) {
        pcl::PointXYZ p;
        p.x = point.x; p.y = point.y;
        cloud_2d->push_back(p);
    }
//    //生成占据栅格地图
//    //计算地图范围：确定点云在XY平面的最小/最大坐标，计算栅格数量：

//    double resolution = 0.05;
//    pcl::PointType min, max;
//    pcl::getMinMax3D(*cloud_2d, min, max);
//    int width = std::ceil((max.x - min.x) / resolution);
//    int height = std::ceil((max.y - min.y) / resolution);

//    //填充OccupancyGrid数据：遍历点云，标记占据栅格（概率设为80-100），空闲栅格设为0：

//    nav_msgs::OccupancyGrid map;
//    map.info.resolution = resolution; // 栅格分辨率
//    map.info.width = width;
//    map.info.height = height;
//    map.info.origin.position.x = min.x;
//    map.info.origin.position.y = min.y;
//    for (const auto& point : *cloud_2d) {
//        int idx_x = std::floor((point.x - min.x) / resolution);
//        int idx_y = std::floor((point.y - min.y) / resolution);
//        int index = idx_y * width + idx_x;
//        map.data[index] = 80; // 占据概率80%
//    }

    //转换为栅格地图数据并发布
    auto func = [](const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                        nav_msgs::OccupancyGrid &msg) {

        //最小和最大高度
        double thre_z_min = 0.3;
        double thre_z_max = 2.0;
        int flag_pass_through = 0;
        double map_resolution = 0.05;
        double thre_radius = 0.1;

      msg.header.seq = 0;
      msg.header.stamp = ros::Time::now();
      msg.header.frame_id = "map";

      msg.info.map_load_time = ros::Time::now();


      msg.info.resolution = map_resolution;

      double x_min, x_max, y_min, y_max;
      double z_max_grey_rate = 0.05;
      double z_min_grey_rate = 0.95;
      //? ? ??
      double k_line =
          (z_max_grey_rate - z_min_grey_rate) / (thre_z_max - thre_z_min);
      double b_line =
          (thre_z_max * z_min_grey_rate - thre_z_min * z_max_grey_rate) /
          (thre_z_max - thre_z_min);

      if (cloud->points.empty()) {
        ROS_WARN("pcd is empty!\n");
        return;
      }

      for (int i = 0; i < cloud->points.size() - 1; i++) {
        if (i == 0) {
          x_min = x_max = cloud->points[i].x;
          y_min = y_max = cloud->points[i].y;
        }

        double x = cloud->points[i].x;
        double y = cloud->points[i].y;

        if (x < x_min)
          x_min = x;
        if (x > x_max)
          x_max = x;

        if (y < y_min)
          y_min = y;
        if (y > y_max)
          y_max = y;
      }
      // origin的确定
      msg.info.origin.position.x = x_min;
      msg.info.origin.position.y = y_min;
      msg.info.origin.position.z = 0.0;
      msg.info.origin.orientation.x = 0.0;
      msg.info.origin.orientation.y = 0.0;
      msg.info.origin.orientation.z = 0.0;
      msg.info.origin.orientation.w = 1.0;
      //设置栅格地图大小
      msg.info.width = int((x_max - x_min) / map_resolution);
      msg.info.height = int((y_max - y_min) / map_resolution);
      //实际地图中某点坐标为(x,y)，对应栅格地图中坐标为[x*map.info.width+y]
      msg.data.resize(msg.info.width * msg.info.height);
      msg.data.assign(msg.info.width * msg.info.height, 0);

      ROS_INFO("data size = %d\n", msg.data.size());

      for (int iter = 0; iter < cloud->points.size(); iter++) {
        int i = int((cloud->points[iter].x - x_min) / map_resolution);
        if (i < 0 || i >= msg.info.width)
          continue;

        int j = int((cloud->points[iter].y - y_min) / map_resolution);
        if (j < 0 || j >= msg.info.height - 1)
          continue;
        // 栅格地图的占有概率[0,100]，这里设置为占据
        msg.data[i + j * msg.info.width] = 100;
        //    msg.data[i + j * msg.info.width] = int(255 * (cloud->points[iter].z *
        //    k_line + b_line)) % 255;
      }


    };


    func(m_cloud, m_msg);

    m_rviz->Set_Enable(QString::fromStdString(TopicFiled::map),true);

    GRosNode->pubMap(m_msg);

    m_checkMap->setCheckState(Qt::CheckState::Checked);
}

void GridMapWidget::onSaveMap()
{
    if(m_fileName == "" || m_msg.data.size() == 0)
    {
        Toast::toast(Toast::Warn,"没有要保存的珊格地图!",this);
        return;
    }

    QString base_name = QFileInfo(m_fileName).baseName();

    QString yaml_file = LeiliCfg->getSavePath() + "/" + base_name + ".yaml";
    QString pgm_file = LeiliCfg->getSavePath() + "/" + base_name + ".pgm";

    ros_map_edit::MapFileManager mgr;

    bool succ = mgr.saveMap(yaml_file.toStdString(), m_msg);
    if(!succ)
    {
        Toast::toast(Toast::Warn,"保存yaml文件失败!",this);
        return;
    }

    succ = mgr.saveMap(pgm_file.toStdString(), m_msg);
    if(!succ)
    {
        Toast::toast(Toast::Warn,"保存pgm文件失败!",this);
        return;
    }

    Toast::toast(Toast::Succ,"保存珊格地图成功!",this);
}
